// =====================================================================================
//
//       Filename:  kdtree.h
//
//    Description:  Définition d'un kd-tree (arbre de dimension k), structure de données
//					de partition spatiale appropriée pour le calcul du plus proche
//					voisin
//
//        Version:  1.0
//        Created:  11/08/2009 09:09:00
//       Revision:  none
//       Compiler:  g++
//
//         Author:  François Hissel (fh), francois.hissel@m4x.org
//        Company:
//
// =====================================================================================

#ifndef KDTREE_H_INC
#define KDTREE_H_INC

#include	<vector>
#include	<algorithm>
#include	<cfloat>

using namespace std;

/**
 * \fn bool true_condition(const TPoint *p)
 * \brief Fonction de condition qui retourne toujours vrai
 *
 * La fonction est utilisée pour la définition d'une condition par défaut que doit vérifier un point lors de la recherche du plus proche voisin. Cette condition particulière retourne toujours vrai, ce qui permet de recherche le plus proche voisin parmi tous les points du réseau sans exception. Elle est utilisée comme paramètre par défaut de la fonction membre Kd_tree::neighbour()
 * \param p Point pour lequel la condition est vérifiée, mais il n'est pas utilisé dans cette fonction particulière et ne sert qu'à assurer la concordance du type de la fonction avec le type attendu
 * \return Toujours true
 */
template<class TPoint>
static bool true_condition(const TPoint *p) {
	return true;
}

/**
 * \class Kd_tree
 * \brief Structure d'arbre de dimension \e k, utilisée pour le calcul du plus proche voisin en temps optimal
 *
 * La classe est une représentation d'un ensemble de points d'un espace de dimension \e k sous la forme d'un arbre binaire qui partage l'espace en des hyper-rectangles, de façon à accélérer la recherche du plus proche voisin.
 * L'arbre est une feuille si ses deux fils ont la valeur 0 (pointeur nul).
 * \n Voir aussi <em>"Multidimensional divide and conquer. Communications of the ACM, 1980. J.L. Bentley"</em> pour plus d'informations sur les kd-trees.
 * \tparam k Dimension de l'arbre
 * \tparam TPoint Type utilisé pour la représentation des noeuds de l'arbre
 * \tparam coords Fonction qui reçoit comme premier argument un pointeur vers une structure TPoint (anonymisé) et retourne dans son second argument un tableau de taille k contenant les coordonnées du point spécifié
 * \tparam square_distance Fonction qui reçoit comme premier argument un pointeur vers une structure TPoint (anonymisé) et retourne le carré de la distance de l'instance au point donné en second argument
 * \warning Aucune vérification n'est faite par le compilateur sur la taille du tableau retourné. Il appartient au programmeur de s'assurer que la classe TPoint définit bien une fonction coords (dont le nom peut être différent) qui renvoie un tableau contenant exactement \e k nombres réels.
 * \warning Pour que l'algorithme de recherche du plus proche voisin fonctionne, la distance définie par la fonction donnée en argument du modèle doit définir une distance supérieure à celle de Tchebychev, c'est-à-dire telle que \f[ \forall (a,b) \in E^2\hbox{,}\quad d(a,b)\geq d_{\infty}(a,b) \f]
 */
template<int k,class TPoint,void (*coords)(const void*,double*),double (*square_distance)(const void*,const double*)>
class Kd_tree {
	public:
		TPoint *point() {return _point;}	//!< Procédure d'accès au point contenu dans la structure
		int separation;	//!< Dimension selon laquelle la coupure est réalisée
		//double fmin[k];	//!< Tableau des coordonnées minimales de l'hyperrectangle
		//double fmax[k];	//!< Tableau des coordonnées maximales de l'hyperrectangle
		Kd_tree<k,TPoint,coords,square_distance> *left;	//!< Pointeur vers le fils gauche de l'arbre
		Kd_tree<k,TPoint,coords,square_distance> *right;	//!< Pointeur vers le fils droit de l'arbre

		/**
		 * \brief Constructeur standard, avec initialisation des champs à la valeur nulle
		 *
		 * Le constructeur initialise les deux fils à la valeur nulle.
		 */
		Kd_tree():left(0),right(0) {}

		/**
		 * \brief Constructeur d'un arbre de dimension k avec initialisation de ses champs
		 *
		 * Le constructeur alloue l'espace nécessaire à l'arbre et initialise tous ses champs.
		 * \param ppoint Point contenu au noeud courant
		 * \param pseparation Numéro de la dimension par laquelle la coupure est réalisée
		 * \param pleft Fils gauche du noeud courant, lui-même un kd-tree de même dimension
		 * \param pright Fils droit du noeud courant, lui-même un kd-tree de même dimension
		 */
		Kd_tree(TPoint *ppoint,int pseparation,Kd_tree<k,TPoint,coords,square_distance> *pleft,Kd_tree<k,TPoint,coords,square_distance> *pright):separation(pseparation),left(pleft),right(pright) {set_point(ppoint);}

		/**
		 * \brief Destructeur standard
		 *
		 * Le destructeur libère la mémoire occupée par les fils gauche et droit si celle-ci est utilisée.
		 */
		~Kd_tree();

		/**
		 * \brief Construit un arbre de recherche à partir d'une liste de points
		 *
		 * La fonction construit progressivement l'arbre de recherche, en ajoutant à chaque étape un point et en divisant le domaine en deux sous-domaines en utilisant une fonction choisie. Le calcul de l'arbre a pour complexité moyenne
		 * \f$ kn \ln^2(n) \f$.
		 * \param points Liste des points à partir desquels construire l'arbre
		 */
		Kd_tree(vector<TPoint*> points);

		/**
		 * \brief Fonction de définition des coordonnées du point représenté par l'instance
		 *
		 * La fonction permet l'écriture du point contenu dans la structure. Elle est utilisée pour cacher l'implémentation de ce point, et permettre d'effectuer des traitements préalables lorsque le contenu est modifié.
		 * \param ppoint Nouvelle valeur du point contenu
		 */
		void set_point(TPoint *ppoint);

		/**
		 * \brief Recherche du plus proche voisin d'un point qui vérifie une certaine condition à partir de l'arbre
		 *
		 * La fonction recherche dans l'arbre le plus proche voisin qui vérifie la condition fixée du point dont les coordonnées sont données en argument. L'opération s'effectue avec une complexité dans le pire des cas de \f$ O\left(kn^{1-\frac{1}{k}}\right) \f$. En moyenne, la complexité de la recherche est ln(n).
		 * L'adresse du plus proche voisin est retournée tandis que la valeur de dist est mise à jour au carré de la distance à ce sommet.
		 * \param ppoint Point de référence à partir duquel le plus proche voisin est recherché
		 * \param dist Carré de la distance au plus proche voisin
		 * \param condition Condition que doit vérifier la solution
		 * \return Pointeur vers le plus proche voisin
		 */
		TPoint *neighbour(const double *ppoint,double &dist,bool (*condition)(const TPoint*)=true_condition) const;

		/**
		 * \brief Recherche des n plus proches voisins d'un point qui vérifient une certaine condition à partir de l'arbre
		 *
		 * La fonction recherche dans l'arbre les n plus proches voisins qui vérifient la condition fixée du point dont les coordonnées sont données en argument. L'opération s'effectue avec une complexité dans le pire des cas de \f$ O\left(kn^{1-\frac{1}{k}}\right) \f$. En moyenne, la complexité de la recherche est ln(n).
		 * Elle prend en paramètre deux tableaux, l'un qui accueillera les adresses des plus proches voisins trouvés, l'autre qui contiendra les distances à ces plus proches voisins. Les tableaux sont complétés à partir du début, et triés dans l'ordre croissant des distances au réseau des points. Si l'algorithme ne trouve pas suffisamment de points, les espaces vacants dans le tableau found prendront la valeur nulle.
		 * \param n Nombre de plus proches voisins à rechercher
		 * \param ppoint Point de référence à partir duquel les plus proches voisins sont recherchés
		 * \param found Tableau des plus proches voisins trouvés. Il doit être alloué avant d'être passé en paramètre à cette fonction.
		 * \param dist Tableau des carrés des distances aux plus proches voisins. Il doit être alloué avant d'être passé en paramètre à cette fonction.
		 * \param condition Condition que doivent vérifier les solutions
		 */
		void neighbours(int n,const double *ppoint,TPoint **found,double *dist,bool (*condition)(const TPoint*)=true_condition) const;
	private:
		TPoint *_point;	//!< Point contenu dans la structure
		double x[k];	//!< Coordonnées du point courant sauvegardées en mémoire
		/**
		 * \brief Fonction intermédiaire récursive pour la recherche du plus proche voisin d'un point à partir de l'arbre, qui vérifie une condition donnée
		 *
		 * La fonction recherche dans l'arbre le plus proche voisin du point dont les coordonnées sont données en argument, et dont la distance est inférieure strictement à dist. L'opération s'effectue avec une complexité dans le pire des cas de \f$ O\left(kn^{1-\frac{1}{k}}\right) \f$. En moyenne, la complexité de la recherche est ln(n).
		 * L'algorithme met à jour le paramètre dist avec la valeur de la distance du voisin le plus proche.
		 * \param ppoint Point de référence à partir duquel le plus proche voisin est recherché
		 * \param dist Carré de la distance du point de référence au sommet le plus proche trouvé
		 * \param condition Pointeur vers une fonction qui indique si la condition sur le point est vérifiée
		 * \return Pointeur vers le plus proche voisin
		 */
		TPoint *neighbour_rec(const double *ppoint,double &dist,bool (*condition)(const TPoint*)) const;

		/**
		 * \brief Fonction intermédiaire récursive pour la recherche des n plus proches voisins d'un point à partir de l'arbre, qui vérifient une condition donnée
		 *
		 * La fonction recherche dans l'arbre les n plus proches voisins du point dont les coordonnées sont données en argument, et dont la distance est inférieure strictement à dist. L'opération s'effectue avec une complexité dans le pire des cas de \f$ O\left(kn^{1-\frac{1}{k}}\right) \f$. En moyenne, la complexité de la recherche est ln(n).
		 * L'algorithme maintient un tableau des n plus proches voisins trouvés jusqu'au point d'exécution ainsi qu'un autre tableau contenant les distances à ces voisins.
		 * \param n Nombre de plus proches voisins à rechercher
		 * \param ppoint Point de référence à partir duquel le plus proche voisin est recherché
		 * \param found Tableau des sommets trouvés jusqu'à présent
		 * \param dist Tableau des carrés des distances du point de référence aux sommets les plus proches trouvés
		 * \param condition Pointeur vers une fonction qui indique si la condition sur le point est vérifiée
		 */
		void neighbours_rec(int n,const double *ppoint,TPoint **found,double *dist,bool (*condition)(const TPoint*)) const;
};

/*
 * ----------------------------------------------------------------------------------------
 * IMPLEMENTATION DE LA CLASSE KD_TREE
 * Cette implémentation doit être placée dans le fichier d'en-têtes à cause de l'utilisation des templates, même si cela nuit à la lisibilité du code.
 * ----------------------------------------------------------------------------------------
 */

static int c_dim;	//!< Variable utilisée pour indiquer la dimension selon laquelle la comparaison de deux points doit être effectuée dans la procédure compare_dim

/**
 * \fn template<int k,class TPoint> bool compare_dim(TPoint *a,TPoint *b)
 * \brief Fonction de comparaison entre deux structures points
 *
 * La fonction compare les points a et b donnés en arguments selon leur dimension donnée par la variable statique c_dim. Elle est utilisée pour la recherche de la médiane dans une direction particulière.
 * \tparam k Dimension de l'espace vectoriel des points de la classe TPoint
 * \tparam TPoint Type de structure pour laquelle la comparaison est effectuée
 * \tparam coords Nom de la fonction qui permet de placer dans son second argument le tableau des coordonnées de son premier argument
 * \param a Premier point à comparer
 * \param b Second point à comparer
 * \return Vrai si \f$ a_k<b_k \f$ et faux sinon
 */
template<int k,class TPoint,void (*coords)(const void*,double*)>
bool compare_dim(TPoint *a,TPoint *b) {
	double co1[k];
	double co2[k];
	coords(a,co1);
	coords(b,co2);
	return co1[c_dim]<co2[c_dim];
}

template<int k,class TPoint,void (*coords)(const void*,double*),double (*square_distance)(const void*,const double*)>
Kd_tree<k,TPoint,coords,square_distance>::~Kd_tree() {
	delete left;
	delete right;
}

template<int k,class TPoint,void (*coords)(const void*,double*),double (*square_distance)(const void*,const double*)>
Kd_tree<k,TPoint,coords,square_distance>::Kd_tree(vector<TPoint*> points) {
	//Eviction du cas où la liste de points est vide
	if (points.size()==0) {
		_point=0;
		left=0;
		right=0;
		return;
	}
	//Résolution du cas d'initialisation de la récurrence où la liste de points contient un unique élément
	if (points.size()==1) {
		set_point(points[0]);
		left=0;
		right=0;
		return;
	}
	//Recherche de la dimension selon laquelle la distribution est la plus étendue
	double maxspread=0;
	separation=0;
	for (int i=0;i<k;++i) {
		double min=DBL_MAX;
		double max=-DBL_MAX;
		for (typename vector<TPoint*>::iterator it=points.begin();it!=points.end();it++) {
			double co[k];
			coords(*it,co);
			if (co[i]<min) min=co[i];
			if (co[i]>max) max=co[i];
		}
		double spread=max-min;
		if (spread>maxspread) {
			maxspread=spread;
			separation=i;
		}
	}
	//Calcul du point médian selon la direction de séparation
	int index_median=points.size()/2;
	c_dim=separation;
	nth_element(points.begin(),points.begin()+index_median-1,points.end(),compare_dim<k,TPoint,coords>);
	//Le point médian est maintenant situé à la position index_median dans la structure de données points. De plus, tous les éléments inférieurs sont situés dans la première moitié du tableau et tous les éléments supérieurs sont situés dans la seconde moitié.
	set_point(points[index_median]);
	vector<TPoint*> lvalues(points.begin(),points.begin()+index_median);
	left=new Kd_tree<k,TPoint,coords,square_distance>(lvalues);
	if (points.size()>2) {
		vector<TPoint*> rvalues(points.begin()+index_median+1,points.end());
		right=new Kd_tree<k,TPoint,coords,square_distance>(rvalues);
	} else right=0;
}

template<int k,class TPoint,void (*coords)(const void*,double*),double (*square_distance)(const void*,const double*)>
void Kd_tree<k,TPoint,coords,square_distance>::set_point(TPoint *ppoint) {
	_point=ppoint;
	coords(_point,x);
}

template<int k,class TPoint,void (*coords)(const void*,double*),double (*square_distance)(const void*,const double*)>
TPoint *Kd_tree<k,TPoint,coords,square_distance>::neighbour_rec(const double *ppoint,double &dist,bool (*condition)(const TPoint*)) const {
	TPoint* res=0;	//Point le plus proche
	double dl=dist;
	if (condition(_point)) {
		dl=square_distance(_point,ppoint);	//Distance du noeud ou du sous-arbre
		if (dl<dist) {
			dist=dl;
			res=_point;
		} else dl=dist;
	}
	double tmp;
	if (left!=0 || right!=0) {
		tmp=ppoint[separation]-x[separation];
		tmp*=tmp;
	}
	if (left!=0 && (ppoint[separation]<=x[separation] || tmp<dl)) {
		TPoint *res2=left->neighbour_rec(ppoint,dl,condition);
		if (res2) {
			dist=dl;
			res=res2;
		} else dl=dist;
	}
	if (right!=0 && (ppoint[separation]>=x[separation] || tmp<dl)) {
		TPoint *res2=right->neighbour_rec(ppoint,dl,condition);
		if (res2) {
			dist=dl;
			res=res2;
		} else dl=dist;
	}
	//A ce stade, res contient le plus proche voisin dans le sous-arbre issu du noeud en cours, et dc contient la distance à ce plus proche voisin
	return res;
}

template<int k,class TPoint,void (*coords)(const void*,double*),double (*square_distance)(const void*,const double*)>
void Kd_tree<k,TPoint,coords,square_distance>::neighbours(int n,const double *ppoint,TPoint **found,double *dist,bool (*condition)(const TPoint*)) const {
	for (int i=0;i<n;++i) {
		dist[i]=DBL_MAX;
		found[i]=0;
	}
	neighbours_rec(n,ppoint,found,dist,condition);
}

template<int k,class TPoint,void (*coords)(const void*,double*),double (*square_distance)(const void*,const double*)>
TPoint *Kd_tree<k,TPoint,coords,square_distance>::neighbour(const double *ppoint,double &dist,bool (*condition)(const TPoint*)) const {
	TPoint *found[1];
	double d[1];
	neighbours(1,ppoint,found,d,condition);
	dist=d[0];
	return found[0];
}

template<int k,class TPoint,void (*coords)(const void*,double*),double (*square_distance)(const void*,const double*)>
void Kd_tree<k,TPoint,coords,square_distance>::neighbours_rec(int n,const double *ppoint,TPoint **found,double *dist,bool (*condition)(const TPoint*)) const {
	double dl;
	if (condition(_point)) {
		dl=square_distance(_point,ppoint);	//Distance du noeud ou du sous-arbre
		if (dl<dist[n-1]) {
			int j=n-2;
			while (j>=0 && dl<dist[j]) {
				dist[j+1]=dist[j];
				found[j+1]=found[j];
				--j;
			}
			dist[j+1]=dl;
			found[j+1]=_point;
		}
	}
	double tmp;
	if (left!=0 || right!=0) {
		tmp=ppoint[separation]-x[separation];
		tmp*=tmp;
	}
	if (left!=0 && (ppoint[separation]<=x[separation] || tmp<dist[n-1])) {
		left->neighbours_rec(n,ppoint,found,dist,condition);
	}
	if (right!=0 && (ppoint[separation]>=x[separation] || tmp<dist[n-1])) {
		right->neighbours_rec(n,ppoint,found,dist,condition);
	}
}

#endif /* KDTREE_H_ */
